/*
 *  constraint.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 25/08/2011
 *
 */

#include "constraint.h"

Constraint::Constraint()
{}

// ***

Constraint::Constraint(vector<string> vp, int di)
:	dim(di),
	pointNames(vp),
	perturbation(1.0e-6),
	penalty(1.0e8),
	isQuadratic(false),
	isContact(false),
	isScalar(false)
{}

// ***

vector<string>& Constraint::getPointNames()
{
	return(pointNames);
}

// ***

void Constraint::setMPoints(map<string,Point*,less<string> > mp)
{
	mPoints=mp;
}

// ***

void Constraint::reportContent()
{
	map<string,Point*,less<string> >::iterator i;
	for(i=mPoints.begin(); i!=mPoints.end(); i++)	{
		cout << "								" << ((*i).second)->name << " at process\n" ;
	}
}

// ***

void Constraint::fillResidual(vector<double>& res_in, const vector<double>& q_in, const vector<double>& dq_in, const double& t, const int& modeFlag, const double& grav, const int& gravDir, const double& DeltaT)
{
	int i;
	double coef;
	time = t;
	
	getLocalDofs(q_in, dq_in);
	
	// modeFlag=0.  If possible, residual and tangent are calculated simultaneously. This is the case for the brick element with an implicit method
	// modeFlag=1.  Just residual is computed. This is the case of an explicit calculation, or an implicit calculation with numerical tangent
	if (isContact==false) calcFq(modeFlag);
	else calcFqC(modeFlag);
	
	deltaT = DeltaT;
	
	// Add the Lagrange multiplier term

	if (augLagFlag==true)
	{
//		if (lTwoNorm(Fq)!=0.0)
//		if (lTwoNorm(Fq)>=1.0e-6)
		if (Fq.norm()>=1.0e-6)
		{
			//FqRatio = lTwoNorm(FqPenalty)/lTwoNorm(Fq);
			FqRatio = FqPenalty.norm()/Fq.norm();
		}
		else 
		{
//			if (lTwoNorm(FqPenalty)!=0.0) FqRatio = 1.0e12;	else FqRatio=0.0;
			FqRatio = 0.0;
		}
		//for(i=0; i<numDofs; i++)	Fq[i]+= FqLag[i];
	}
	
	//calcFt(t,grav,gravDir);
	
	//cout << "IntegratorTye = " << integratorType << endl;

	if(integratorType==0) coef = 1.0; 
	else coef = DeltaT;

	if(Fq.size()!=0)
	{
		for(i=0; i< numDofs; i++)	res_in[ localGlob[i] ]+= coef*Fq(i);
	}
	/*
	cout << "Fq es " << endl;
	cout << Fq << endl;
	*/
//	cout << "q es = "  << endl;
//	cout << q << endl;
//	cout << "Fq es = " << endl;
//	cout << Fq << endl;
	
}

// ***

void Constraint::fillTangent(sparseMatrix& Kint, vector<vector<double> >& Kbb, vector<vector<double> >& Kib, vector<vector<double> >& Kbi, 
							vector<int>& globalLocalI, vector<int>& globalLocalB,
							const vector<double>& q_in,
							 const vector<double>& dq_in,
							const double& time, const int& modeFlag, const double DeltaT)
{
	int i;
	int j;
	
	int a;
	int b;
	double coef;
	
	if(integratorType==0) coef = 1.0; 
	else coef = DeltaT;
	
	getLocalDofs(q_in, dq_in);
	
	if(modeFlag==1)		// Numerical tangent
	{
		calcKqNumeric();
	} else
	{
		if (isContact==false) calcKq(modeFlag);
		else calcKqC(modeFlag);
	}
	
	/*
	cout << "Kq is " << endl;
	cout << Kq << endl;
	*/
	
	if(Kq.size()!=0)
	{
		if (Kbb.size()!=0)
		{
			for(i=0; i< numDofs; i++)
			{
				a = globalLocalI[ localGlob[i] ];	// a!=-1 means interior dof
				for(j=0; j< numDofs; j++)
				{
					b =	globalLocalI[ localGlob[j] ];	// b!=-1 means interior dof
					if (a!=-1 && b!=-1)	// Belongs to Kint
					{
					//cout << "belongs to Kint " << endl;
						Kint.writeElement( Kint.readElement(a,b) + coef*Kq(i,j), a, b );
					}
					else if (a==-1 && b==-1)	// Belongs to Kbb
					{
						//cout << "belongs to Kbb " << endl;
						Kbb[ globalLocalB[ localGlob[i] ] ][ globalLocalB[ localGlob[j] ] ] += coef*Kq(i,j);
					}
					else if (a!=-1 && b==-1)	// Belongs to Kib
					{
						//cout << "belongs to Kib " << endl;
						Kib[ globalLocalI[ localGlob[i] ] ][ globalLocalB[ localGlob[j] ] ] += coef*Kq(i,j);
					}
					else if ( (a==-1 && b!=-1) && Kbi.size()!=0 )	// Belongs to Kbi
					{
						//cout << "belongs to Kbi " << endl;
						Kbi[ globalLocalB[ localGlob[i] ] ][ globalLocalI[ localGlob[j] ] ] += coef*Kq(i,j);
					}
				}
			}
		}
		else	// Just one process, no boundaries
		{
			for(i=0; i< numDofs; i++)
			{
				for(j=0; j< numDofs; j++)
				{
					//cout << "Kq[" << i << "][" << j << "] = " << Kq[i][j] << endl;
					Kint.writeElement( Kint.readElement(localGlob[i], localGlob[j]) + coef*Kq(i,j), localGlob[i], localGlob[j]);
				}
			}
		}
	}
}

// ***

void Constraint::fillForce(vector<double>& f,
						const vector<double>& q,
						const vector<double>& qdot,
						const double& t,
						const double& grav, const int& gravDir
						)
{
	int i;
	getLocalDofs(q,qdot);
//	getLocalDofs(q);
	
	if (isContact==false)	calcFq(1);
	else calcFqC(1);

	//if (augLagFlag==true && Fq.size()!=0)	for(i=0; i<numDofs; i++)	Fq[i]+= FqLag[i];
	if (augLagFlag==true && Fq.size()!=0 && FqLag.size()!=0)	Fq +=FqLag;

	//calcFt(t,grav,gravDir);
/*
	cout << " q in fillFOrce is " << endl;
	for(i=0; i<q.size(); ++i)	cout << " " << q[i];
	cout << endl;
	
	cout << "Fq in fillForce is " << endl;
	printSTLVector(Fq);
*/	
	if(Fq.size()!=0)
	{
		for(i=0; i< numDofs; i++)	f[ localGlob[i] ]-= Fq (i);
	}
}

// ***
/*
void Constraint::getLocalDofs(const vector<double> &q_in)
{
	int i;
	if(q.size()!=0)
	{
		for(i=0; i<numDofs; i++) q[i] = q_in[ localGlob[i] ];
	}
}
*/
// ***

void Constraint::getLocalDofs(const vector<double> &q_in, const vector<double> &dq_in)
{
	int i;
	if(q.size()!=0)
	{
		for(i=0; i<numDofs; i++) q(i) = q_in[ localGlob[i] ];
	}
	if(dq.size()!=0)
	{
		for(i=0; i<numDofs; i++)	dq(i) = dq_in[ localGlob[i] ];
	}       
}

// ***

void Constraint::setLocalGlob()
{
	int k;
	Point* p;
	localGlob.resize(0);
//	localGlobInertia.resize(0);
	int i;
	for(i=0; i<pointNames.size(); i++)	/** Loop on points */
	{
		p = mPoints[pointNames[i]];
		for(k=0; k< (p->globalTNum.size()); k++)	/** Firstly, translational */
		{
			localGlob.push_back(p->globalTNum[k]);
		}
		for(k=0; k< (p->globalRNum.size()); k++)	/** Secondly, rotational */
		{
			localGlob.push_back(p->globalRNum[k]);
		}
//		for(k=0; k< (p->globalINum.size()); k++)	/** Dofs that contribute to inertia */
//		{
//			localGlobInertia.push_back(p->globalINum[k]);
//		}		
	}
	
	/*
	// Constraint dofs
	for(k=0; k< globalTNum.size(); k++)
	{
		localGlob.push_back(globalTNum[k]);
	}
	*/
	
	//cout << "globalTNum.size is " << globalTNum.size() << endl;
	//cout << "localGlob.size is " << localGlob.size() << endl;
	//cout << "numDofs is " << numDofs << endl;
	
	
//	if(localGlob.size()!=numDofs) cout << "*WARNING: Potential inconsistency in dofs at object of type Constraint \n";
	/*
	cout << "localGlob.size() = " << localGlob.size() << "\n";
	cout << "numDofs = " << numDofs << "\n";

	for(k=0; k<localGlob.size(); k++) cout << "localGlob[" << k << "]= " << localGlob[k] << "\n";

	cout << "Saliendo de setLocalGlobal dentro de constraint " << endl;
	*/
}

// ***

void Constraint::setSparsePattern(vector< vector<int> >& tempIa)
{
	map<string,Point*,less<string> >::iterator k;
//	Point* p;
	vector<int> vNumbersA;
	vector<int> vNumbersB;
	vector<int> vNumbers;
	int i;
	int j;
	int a;
//	int kk=0;
//	cout << "a coger los " << mPoints.size() << " del Constrainte " << endl;
//	k = mPoints.begin();
//	cout << k->second->name << endl;
	for(k=mPoints.begin(); k!=mPoints.end(); k++)
	{
//		cout << "Voy a coger los numeros del punto " << k->second->name << endl;
		vNumbersA = k->second->getGlobalNumbers();
		vNumbersB.resize(vNumbersA.size()+vNumbers.size());

		merge(vNumbersA.begin(),vNumbersA.end(),vNumbers.begin(),vNumbers.end(),vNumbersB.begin());
		vNumbers = vNumbersB;
		vNumbersB.clear();
//		++kk;
	}
	
	// It is neccessary to include the Constraint dofs (if any)
	/*
	if (globalTNum.size()!=0)
	{
		vNumbersA = globalTNum;
		vNumbersB.resize(vNumbersA.size()+vNumbers.size());

		merge(vNumbersA.begin(),vNumbersA.end(),vNumbers.begin(),vNumbers.end(),vNumbersB.begin());
		vNumbers = vNumbersB;
		vNumbersB.clear();
//		cout << "He cogido un Constraint dof " << endl;
	}
	*/
	for (i=0; i<vNumbers.size(); ++i)	// Loop on global dofs
	{
		a = vNumbers[i];
		for (j=0; j<vNumbers.size(); ++j)
		{
			tempIa[a].push_back( vNumbers[j] );
		}
	}
	//cout << "Aqui el tempIa es " << endl;
	//printSTLMatrix(tempIa);
}

// ***

void Constraint::changePointName(string& oldName, string& newName)
{
	// If oldName is not in pointNames, it does nothing
	replace(pointNames.begin(),pointNames.end(),oldName,newName);
}

// ***

void Constraint::calcKqNumeric()
{
	int i;
	int j;
	
	if(Fq.size()!=0)
	{
		if(Kq.size()==0)
		{
				Kq.setZero(numDofs,numDofs);
		}
		
		for (j=0;j<numDofs; ++j)
		{
			q(j)+= perturbation;
			if (isContact==false) calcFq(1);	// 1 means no simultaneous tangent computation (if available)
			else calcFqC(1);
		
			for (i=0; i<numDofs; ++i)	Kq(i,j) = Fq(i);
		
			q(j)-= 2.0*perturbation;
			if (isContact==false) calcFq(1);	// 1 means no simultaneous tangent computation (if available)
			else calcFqC(1);
			
			for (i=0; i<numDofs; ++i)
            {
                Kq(i,j) = (Kq(i,j)-Fq(i))/(2.0*perturbation);
//                Kq(i,j) /= (2.0*perturbation);
            }
			q(j)+= perturbation;
		}
		if (isContact==false) calcFq(1);	// 1 means no simultaneous tangent computation (if available)
		else calcFqC(1);
		
// 		cout << "Kq es " << endl;
// 		printSTLMatrix(Kq);
		
		// Force block structure
		setKqSymmetries();
		/*
		Kq.block(dim,dim,dim,dim) = Kq.block(0,0,dim,dim);
		Kq.block(0,dim,dim,dim) = -Kq.block(0,0,dim,dim);
		Kq.block(dim,0,dim,dim) = -Kq.block(0,0,dim,dim);;		
		 */
	}
}

// ***

void Constraint::update(const int& iType, const vector<double>& qConv_in, const vector<double>& dqConv_in)
{
	int i;
	integratorType = iType;
	
	if(qC.size()==0)        qC.setZero(numDofs);
	if(dqC.size()==0)       dqC.setZero(numDofs);
	
	for(i=0; i<numDofs; i++)
	{
		qC(i) = qConv_in[ localGlob[i] ];
		dqC(i) = dqConv_in[ localGlob[i] ];
	}
	//if (augLagFlag==true)
	//{
		calcPhi(qC);
		PhiC = Phi;
	//}
	updateCons();   // Update internal variables (if any) at Constraints
}

// ***

void Constraint::updateLagMul(void)
{
	int i;
	
	if(integratorType!=1)	calcPhi(q);
	else
	{
		qMid = 0.5*(q + qC);
		calcPhi(qMid);
	}
	
	/*
	cout << "En updateLagMul, Phi es " << endl;
	cout << Phi << endl;
	
	cout << "En updateLagMul, Lag es ANTES " << endl;
	cout << lagMul << endl;
	*/
	
	if(integratorType!=2)	lagMul += penalty*Phi;
	else	lagMul += penalty*(Phi + PhiC)/2.0;
	
	/*
	if (augLagFlag==true)
	{
		cout << "q is " << endl;
		cout << q << endl;
		
		cout << "Phi is " << endl;
		cout << Phi << endl;
		
		cout << "Lagrange multiplier is " << endl;
		cout << lagMul << endl;
	}
	 */
/*	
	cout << "En updateLagMul, Lag es DESPUES " << endl;
	printSTLVector(vLagMul);
*/
}

// ***

void Constraint::checkConvLagMul(bool& c)
{
	double tol = 1.0e-6;

	//cout << "FqRatio = " << FqRatio << endl;
	
	if (FqRatio >= tol)	c = false;	// no converged
}

// ***

void Constraint::calcProjMatrix(sparseMatrix& A, double& projPenal)
{
	int i;
	int j;
	int k;
	double aux;
	/*
	cout << "Constraint receives projection matrix " << endl;
	for (i=0; i<A.rows(); ++i)
	{
		for (j=0; j<A.rows(); ++j)
		{
			cout << A.readElement(i, j) << " ";
		}
		cout << endl;
	}
	cout << endl;
	*/
	calcDPhi(q);

	if (dPhi.size()!=0)
	{			
		// For just one constraint equation, dPhi is a row matrix
		for (i=0; i<dPhi.size(); ++i)
		{
			for (j=0; j<dPhi.size(); ++j)
			{
				aux = projPenal*dPhi(i)*dPhi(j);
				if (aux!=0.0)
				{
					A.writeElement(A.readElement(localGlob[i], localGlob[j]) + aux, localGlob[i], localGlob[j]);
				}
			}
		}
	}	
}	

// ***

void Constraint::calcCon(vector<double>& vcon, vector<double>& q_in, vector<double>& dq_in, vector<double>& ddq_in, double& time_in)
{
	int i;

	double aux;
	VectorXd auxq(numDofs);
	
	for (i=0; i<numDofs; ++i)	auxq[i] = q_in[localGlob[i]];
	
	// Constraint (Square of the modulus)
	calcPhi(auxq);
	vcon[0] = fabs(Phi);
	
	// Velocity constraint	dPhi*dq+dPhit
	// dPhit is the partial derivative of the constraint with time. TO BE DONE
	calcDPhi(auxq);
	aux = 0.0;
	for (i=0; i<dPhi.size(); ++i)
	{
		aux+= dPhi(i)*dq_in[localGlob[i]];
	}
	vcon[1]+= aux*aux;
	
	// Acceleration constraint	dot-dPhi*dq + dPhi*ddot-q + ddPhit
	// TO BE DONE
}	

// ***

void Constraint::fillPhiq(vector<vector<double> >& Phiq, int& numd)
{
	int i;
	int j;
	vector<double> aux(numd);
	
	calcDPhi(q);
	
	setZero(aux);
	for (j=0; j<numDofs; ++j)
	{
		aux[localGlob[j]] = dPhi(j);
	}
	Phiq.push_back(aux);	
}

// ***

void Constraint::setPointNames(vector<string>& pNames)
{
	pointNames = pNames;
}

// ***

void Constraint::calcFq(const int&)
{
	int i;

	if(integratorType==0)	// Standard integrator
	{
		// Fpenalty = penalty*Phi*dPhi;
		calcPhi(q);
		calcDPhi(q);
		
		FqPenalty = penalty*Phi*dPhi;

		if(augLagFlag==true)	FqLag = lagMul*dPhi;
	}
	else if(integratorType==1)	// Midpoint
	{
		if (qMid.size()==0)	qMid.setZero(numDofs);
		qMid = (q + qC )/2.0;
		calcDPhi(qMid);
		calcPhi(qMid);
		
		FqPenalty = penalty*Phi*dPhi;
		
		if(augLagFlag==true)	FqLag = lagMul*dPhi;
	}
	
	if(integratorType==2)		// Conservative
	{	
		calcPhi(q);
		averagePhi = (Phi + PhiC)/ 2.0;
		
		calcDiscreteDPhi(qC,q);
		
		FqPenalty = penalty*averagePhi*discretedPhi;
	
		if(augLagFlag==true)
		{
			FqLag = lagMul*discretedPhi;
		}
	}
	
	// Total force
	Fq = FqPenalty + FqLag;
	setFqSymmetries();
	/*
	cout << "Fq dentro de constraint es " << endl;
	cout << Fq << endl;
	*/
}

// ***

void Constraint::calcKq(int)
{

	if(integratorType==0)	// Standard integrator
	{
		calcPhi(q);
		calcDPhi(q);
		calcDDPhi(q);
        if (Phi!=0.0)
        {
            Kq = penalty* ( dPhi*dPhi.transpose() + Phi*ddPhi );
        }
        else  calcKqNumeric();
        
		if (augLagFlag==true)
		{
			Kq += lagMul*ddPhi;
		}
	}
	else if(integratorType==1)	// Midpoint
	{
		if (qMid.size()==0)	qMid.setZero(numDofs);
		qMid = (q + qC )/2.0;
		
		calcPhi(qMid);
		calcDPhi(qMid);
		calcDDPhi(qMid);
		Kq = 0.5*penalty* ( dPhi*dPhi.transpose() + Phi*ddPhi );
		if (augLagFlag==true)
		{
			Kq += 0.5*lagMul*ddPhi;
		}				
	}
	if (integratorType==2)	// Conservative
	{
		// Kq is here the derivative of the discrete derivative respect to q
		Kq = calcDDiscreteDPhi(qC,q);
			
		if (augLagFlag==true)
		{
			Kq *= ( penalty * averagePhi + lagMul);
		}
		else
		{
			Kq *= penalty * averagePhi;
		}
		calcDPhi(q);
		Kq += 0.5*penalty*discretedPhi*dPhi.transpose();
	}
	setKqSymmetries();
	
	// Just in case there are 0 at the diagonal, introduce an artificial small stiffness
	int i;
	for (i=0; i<numDofs; ++i)
	{
		//cout << "Kq(" << i << "," << i << ") = " << Kq(i,i) << endl;
		if (fabs(Kq(i,i))==0) Kq(i,i) = 1.0e-6;
	}
}

// ***

void Constraint::calcKdq(double&)
{}	

// ***

void Constraint::initializeConsRes(const bool& st, const double& grav, const int& gravDir, const bool augl)
{
	Fq.setZero(numDofs);
	if (isContact==false)
	{
		augLagFlag = augl;
		lagMul = 0.0;
		FqLag.setZero(numDofs);
		FqPenalty.setZero(numDofs);
	
		dPhi.setZero(numDofs);
		deltaQ.setZero(numDofs);					   
	}
	else initializeConsResC(st, grav, gravDir, augl);
}

// ***

void Constraint::initializeConsTang(const bool& bo)
{
	
	Kq.setZero(numDofs,numDofs);
	if (isContact==false)
	{
		ddPhi.setZero(numDofs,numDofs);
	}
	else initializeConsTangC(bo);
}

// ***

void Constraint::calcEner(double& Ekin, double& Epot, double& Econ, double& Eext, double& Eamb, double& Dint, double& grav, int& gravDir, vector<double>& q_in, vector<double>& dq_in)
{
	if (isContact==false)
	{	
		int i;
		for (i=0; i<numDofs; ++i)	q(i) = q_in[localGlob[i]];
		calcPhi(q);
		Econ += 0.5*penalty*Phi*Phi;
	}
	else calcEnerC(Ekin, Epot, Econ, Eamb, grav, gravDir, q_in, dq_in);
}

// ***

void Constraint::initializeGlobalDofs(vector<double> &q_in, vector<double> &dq_in, const double & t)
{
	int i;
	int j;
	int p;
	
	// Initial position

	q.setZero(numDofs);

	i=0;
	for(p=0; p<pointNames.size(); ++p)
	{
		for(j=0; j<dim; j++)
		{
			// Default: initial position dofs are point coordinates
			// they go to the firsts positions
			q_in[ localGlob[i*dim+j] ] = mPoints[pointNames[p]]->r[j];
		}
		i++;
	}
	
	// WARNING:
	// Remember that the ordering of vector pointNames and map mPoints is different
	
	dq.setZero(numDofs);
}

// ***

void Constraint::setDofs(void)
{
	// It must not add any dof, but must set variable numDofs
	map<string,Point*,less<string> >::iterator k;
	
	numDofs = 0;
	int num;
//	numDofsPoints.resize(mPoints.size());
	for(k=mPoints.begin();k!=mPoints.end(); k++)
	{
		num = k->second->countDofs(); 
		numDofsPoints.push_back(num); 
		numDofs += num;
	}
}

// ***

void Constraint::calcDiscreteDPhi(VectorXd & QC, VectorXd & Q)
{
	if (discretedPhi.size()==0)	discretedPhi.setZero(numDofs);
	
	if (qMid.size()==0)	qMid.setZero(numDofs);
	qMid = (Q + QC )/2.0;

	/*
	// Does work
	// Scalar constraint
	// Partitioned discrete derivative
	if (isScalar==true)
	{
		denom = getScalar(Q)-getScalar(QC);
		if (denom!=0.0) 
		{
			calcPhi(Q);
			discretedPhi = getDScalar(qMid)*(Phi-PhiC)/denom/getDScalar(qMid).norm();
		}
		else 
		{
			calcDPhi(qMid);
			discretedPhi = dPhi;	// Midpoint evaluation
		}
	}
	else	// Non-scalar constraint
	{		
		calcDPhi(qMid);
		
		if (isQuadratic==true)	// At most quadratic
		{
			discretedPhi = dPhi; 
		}
		else
		{
			deltaQ = Q - QC;
			denom = deltaQ.squaredNorm();
			if (denom!=0.0)
			{
				calcPhi(Q);
				midCorrection = ( Phi - PhiC -dPhi.dot(deltaQ) ) / denom;
				discretedPhi = dPhi + midCorrection*deltaQ;
			}
			else 
			{
				discretedPhi = dPhi;
			}
		}
	}
*/
	
	midCorrection = 0.0;
	if (discretedPhi.size()==0)	discretedPhi.setZero(numDofs);
	
	deltaQ = Q - QC;
	
	denom = deltaQ.squaredNorm();

	if (qMid.size()==0)	qMid.setZero(numDofs);
	qMid = (Q + QC )/2.0;
	
	calcPhi(Q);
	calcDPhi(qMid);
	
	if (isQuadratic==false && denom!=0.0 )
	{
		midCorrection = ( Phi - PhiC -dPhi.dot(deltaQ) ) / denom;
	}
	
	discretedPhi = dPhi + midCorrection*deltaQ;
}

// ***

MatrixXd Constraint::calcDDiscreteDPhi(VectorXd & QC, VectorXd & Q)
{

	/*
	// Still does not work
	// Scalar constraint
	// Partitioned discrete derivative

	 if (vaux.size()==0) vaux.setZero(numDofs);
	 qMid = (Q + QC )/2.0;
	 	 
	if (isScalar==true)
	{
		denom = getScalar(Q)-getScalar(QC);
		if (denom!=0.0)
		{
			calcPhi(Q);
			calcDPhi(Q);
			vaux = (dPhi-0.5*discretedPhi)/denom;
			return 0.5*(Phi-PhiC)*getDDScalar(qMid)/denom + getDScalar(qMid)*vaux.transpose();
		}
		else
		{
			calcDDPhi(qMid);
			return 0.5*ddPhi;
		}
	}
	else	// Non-scalar constraint
	{		
		if (isQuadratic==true)	// At most quadratic
		{
		}
		else
		{
		}	
	}
*/

	if (vaux.size()==0) vaux.setZero(numDofs);

	deltaQ = Q - QC;
	qMid = (Q + QC )/2.0;
	calcDDPhi(qMid);
 
	if (isQuadratic==false && deltaQ.squaredNorm()!=0.0)
	{
		vaux = dPhi - (0.5*ddPhi*deltaQ);
		calcDPhi(qMid);
		vaux -= (dPhi + 2.0*midCorrection*deltaQ);
		vaux /= deltaQ.squaredNorm();
		return (0.5*ddPhi + midCorrection*MatrixXd::Identity(numDofs,numDofs) + deltaQ*vaux.transpose() );	
	}
	else 
	{
		return 0.5*ddPhi;
	}
}
